/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.28
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *  @file trajectory.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "trajectoryscale.h"
#include <cmath>

//#define IF_DEBUG true
#define IF_DEBUG false

TrajectoryScale::TrajectoryScale(double _deltaTime, int _stopTime):
    deltaTime(_deltaTime),
    referScale(1),
    scale(1)
{
    setComponentVersion( "TrajectoryScale", "2016-12-9", "1.0.0" );

    scale_dot = referScale / _stopTime;
    currentTime = 0;
    scaleStatus = ENUM_TS_HOLD;
}

void TrajectoryScale::update()
{
    switch(scaleStatus)
    {
        case ENUM_TS_ACC:
        {
            this->linearUp();
            if (scale >= referScale)
            {
                scale = referScale;
                c = 0;
                scaleStatus = ENUM_TS_HOLD;
            }
            break;
        }//case ENUM_TS_ACC
        case ENUM_TS_DEC:
        {
            this->linearDown();
            if(scale <= referScale)
            {
                scale = referScale;
                c = 0;
                if(fabs(referScale)<eps)
                {
                    referScale = 0;
                    scale = 0;
                    scaleStatus = ENUM_TS_CEASE;
                }
                else
                {
                    scaleStatus = ENUM_TS_HOLD;
                }
            }
            break;
        }//case ENUM_TS_DEC
    }
    currentTime += scale * deltaTime;
    a = scale;
    b = a*a;
}

void TrajectoryScale::linearDown()
{
    scale -= scale_dot;

}

void TrajectoryScale::linearUp()
{
    scale += scale_dot;
}

void TrajectoryScale::setScale(double expectScale)
{
    if(expectScale<0)
    {
        expectScale=0;
    }
    else if (expectScale>1)
    {
        expectScale=1;
    }
    if(fabs(expectScale-referScale)>eps)
    {
        if(expectScale>scale)
        {
            scaleStatus = ENUM_TS_ACC;
            c = scale_dot;
        }
        else
        {
            scaleStatus = ENUM_TS_DEC;
            c = -scale_dot;
        }
//        scale_dot *= (expectScale/referScale);
        referScale = expectScale;
    }

}

void TrajectoryScale::resetTime(double _currentTime)
{
    currentTime = _currentTime;
}

void TrajectoryScale::resetScale(double _scale)
{
    if (_scale <= 0.0)//暂停
    {
        scale = 0.0;
        scaleStatus = ENUM_TS_CEASE;
    }
    else//非暂停
    {
        scaleStatus = ENUM_TS_HOLD;
        if (_scale > 1.0)
        {
            scale = 1.0;
        }
       else
        {
            scale = _scale;
        }
    }
    referScale = scale;
    a = scale;
}

void TrajectoryScale::resetScaleDot(int stopTime)
{
    scale_dot = 1.0 / stopTime;;
}

TrajectoryScale::~TrajectoryScale()
{
}
